#define n 5                      
uniform sampler2D     Positions;
uniform sampler2DRect P;
uniform sampler2DRect Q;
uniform sampler3D     WeightsTable;

const float K1DIV3   = 0.3333333;    /// 1/3
const float K1DIV9   = 0.1111111;    /// 1/9
const float K1DIV54  = 0.0185185;    /// 1/54

float cubicroot(in float a, in float b, in float c) {
   
   const float Q = (3.0*b - a*a)*K1DIV9;
   const float R = (9.0*a*b - 27.0*c - 2.0*a*a*a)*K1DIV54;
   const float D = Q*Q*Q + R*R;
   const float sqrtD = sqrt(D);
   
   if (D>=0.0) {  /// one real, two complex :: D==0 root reals, at least two are equal 
      const float S = sign(R+sqrtD)*pow(abs(R+sqrtD), K1DIV3);
      const float T = sign(R-sqrtD)*pow(abs(R-sqrtD), K1DIV3);
      return -a*K1DIV3 + S + T;
   }
   else {   /// all root are real and unequal
      const float theta = acos(R/sqrt(-Q*Q*Q));
      return 2.0*sqrt(-Q)*cos(theta*K1DIV3)-a*K1DIV3;
   }
}

//~ float weight(in vec3 p, in int i) {
   
   //~ vec3 vd = texture2DRect(P, vec2(0.0, i)).xyz - p;
   //~ float denom = dot(vd,vd);
   //~ return 1.0/(denom + 0.01*0.01);
//~ }

vec3 compute_pstar(in vec3 p) {
   
   const vec2 coofrag = gl_TexCoord[0].st;
   float Wt=0.0, W; vec3 r=vec3(0.0);
   for (int i=0; i<n; ++i) {
      float iz = ((i%n)+0.5)/n;
      W = texture3D(WeightsTable, vec3(coofrag, iz)).r;
      Wt += W;
      r  += W*(texture2DRect(P, vec2(0.0, i))).xyz;
   }
   return r/Wt;
}
   
vec3 compute_qstar(in vec3 p) {
   
   const vec2 coofrag = gl_TexCoord[0].st;
   float Wt=0.0, W; vec3 r=vec3(0.0);
   for (int i=0; i<n; ++i) {
      float iz = ((i%n)+0.5)/n;
      W = texture3D(WeightsTable, vec3(coofrag, iz)).r;
      Wt += W;
      r  += W*(texture2DRect(Q, vec2(0.0, i))).xyz;
   }
   return r/Wt;
}

void main() {
   
   const vec2 coofrag = gl_TexCoord[0].st;
   const vec3 p = texture2D(Positions, coofrag).xyz;
   
   vec3 pstar = compute_pstar(p);
   vec3 qstar = compute_qstar(p);
   
   vec3 pjhatT, qjhat; mat3 M = mat3(0.0);
   for (int j=0; j<n; j++) {
      pjhatT  = (texture2DRect(P, vec2(0.0, j))).xyz - pstar;
      qjhat   = (texture2DRect(Q, vec2(0.0, j))).xyz - qstar;
      float iz = ((j%n)+0.5)/n;
      pjhatT = pjhatT*texture3D(WeightsTable, vec3(coofrag, iz)).r;
      M[0] += (pjhatT*qjhat.x);
      M[1] += (pjhatT*qjhat.y);
      M[2] += (pjhatT*qjhat.z);
   }
   
   const float traceM = M[0][0] + M[1][1] + M[2][2];
   const float detM = M[0][0]*(M[1][1]*M[2][2]-M[1][2]*M[2][1])+
                      M[0][1]*(M[1][2]*M[2][0]-M[1][0]*M[2][2])+
                      M[0][2]*(M[1][0]*M[2][1]-M[2][0]*M[1][1]);
   const vec3 V = vec3(M[1][2]-M[2][1], M[2][0]-M[0][2], M[0][1]-M[1][0]);
   vec4 N0 = vec4(M[0][0]+M[0][0], M[1][0]+M[0][1], M[2][0]+M[0][2], V[0]);
   vec4 N1 = vec4(M[0][1]+M[1][0], M[1][1]+M[1][1], M[2][1]+M[1][2], V[1]);
   vec4 N2 = vec4(M[0][2]+M[2][0], M[1][2]+M[2][1], M[2][2]+M[2][2], V[2]);
   
   float normaM[3], normaM2=0.0, prodesc2=0.0, prodNorma2=0.0;
   normaM[0] = dot(M[0],M[0]);
   normaM2 += normaM[0];
   normaM[1] = dot(M[1],M[1]);
   normaM2 += normaM[1];
   normaM[2] = dot(M[2],M[2]);
   normaM2 += normaM[2];
   
   prodNorma2 += normaM[0]*normaM[1];
   prodNorma2 += normaM[1]*normaM[2];
   prodNorma2 += normaM[2]*normaM[0];
   
   float prodesc;
   prodesc = dot(M[0],M[1]);
   prodesc2 += prodesc*prodesc;
   prodesc = dot(M[1],M[2]);
   prodesc2 += prodesc*prodesc;
   prodesc = dot(M[2],M[0]);
   prodesc2 += prodesc*prodesc;
   
   const float a = -4.0*normaM2;
   const float b = 16.0*(prodNorma2-prodesc2);
   const float c = -64.0*detM*detM;
   float p2 = cubicroot(a,b,c); /// cubic root
   
   /// ///////////////////////////////
   
   const float _p = sqrt(p2);
   const float minusp2_plus_4M2 = -p2 + 4.0*normaM2;
   const float _16detMdivp = (16.0*detM)/_p;
   
   /// the largest quartic root
   float maxroot = max(-_p + sqrt(minusp2_plus_4M2 - _16detMdivp), 
                        _p + sqrt(minusp2_plus_4M2 + _16detMdivp));
   maxroot *= 0.5;
   const float eigenvalue = maxroot + traceM;
   
   /// solving the 3x3 linear system: (N-eigenvalue*I) e = V
   N0[0] -= eigenvalue;
   N1[1] -= eigenvalue;
   N2[2] -= eigenvalue;
   N1 -= (N1[0]/N0[0])*N0;
   N2 -= (N2[0]/N0[0])*N0;
   N2 -= (N2[1]/N1[1])*N1;
   const float vez =  N2[3]/N2[2];
   const float vey = (N1[3]-N1[2]*vez)/N1[1];
   const float vex = (N0[3]-N0[2]*vez-N0[1]*vey)/N0[0];
   vec3 u = vec3(vex, vey, vez);
   
   /// Apply the rotation defined by u
   vec3 vhat = p - pstar;
   const vec3 omega = cross(vhat,u);
   const vec3 vd  = vhat - (2.0*(cross(u,omega) + omega))/(1.0+dot(u,u));
   
   gl_FragColor = vec4(qstar+vd, 1.0);
}

/// ///////////////////////////////////////////////////
/*
void main() {
   
   //~ gl_Position = ftransform();
   
   vec3 p = vec3(gl_Vertex.xyz);
   
   vec3 pjhat = (texture2DRect(P, vec2(0.0,0))).xyz;
   vec3 qjhat = (texture2DRect(Q, vec2(0.0,1))).xyz;
   
   // Apply the 4x4 model_view_projection matrix
   gl_Position = gl_ModelViewProjectionMatrix*vec4(p*1.3, pjhat.x + qjhat.y + 1.0);
   //~ gl_FrontColor = vec4(0.0,1.0,0.0,1.0);
}
*/
